/**
 * @file	Test_Beacon_Roomba.c
 * @author	Mitch, David
 * @brief
 */
#include "../header/common.h"
#include "../roomba/roomba.h"
#include "../roomba/roomba_sci.h"
#include "../radio/radio.h"
#include "../uart/uart.h"
#include <util/delay.h>

#ifdef TEST_RADIO_CAT_BUMPER

#define     clock8MHz()    CLKPR = _BV(CLKPCE); CLKPR = 0x00;

typedef enum _ir_dir
{
	IR_NORTH,
	IR_EAST,
	IR_SOUTH,
	IR_WEST,
	IR_NONE,
} IR_Direction_t;

/* RTOS PPP array definitions */
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;

void set_roomba_command(radiopacket_t *packet, uint8_t command, uint8_t arguments[4]);

void init_radio();

/* flag set when receiving packet */
volatile uint8_t rxflag = 0;

/* Mouse address */
uint8_t mouse_addr[5] = { 0xED, 0xB7, 0xED, 0xB7, 0xED };	//0xEDB7EDB7ED

/* Cat address */
uint8_t cat_addr[5] = { 0x12, 0x12, 0x12, 0x12, 0x12 };

/* packet to be sent containing movement command */
radiopacket_t command_packet;

uint8_t down_100_command[4] = {0xFF, 0x9C, 0x80, 0x00};	//backward 100mm/s

void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

int main(void) {

	clock8MHz();

	//initialize leds
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);

	//initialize roomba
	Roomba_Init();

	//disable interrupts
	cli();

	//initializing radio
	init_radio();

	//enable interrupts
	sei();

	//start driving left
	//roomba_Drive(100, 0x0001);

	set_roomba_command(&command_packet, DRIVE, down_100_command);

	Radio_Transmit(&command_packet, RADIO_WAIT_FOR_TX);

	radiopacket_t status_packet;

	int16_t bumper_status = 0;
	int16_t ir_status = 0;

	for (;;) {
		//statement entered upon receiving status packet from roomba
		if (rxflag) {
			_delay_ms(20);

			if (Radio_Receive(&status_packet) != RADIO_RX_MORE_PACKETS) {
				rxflag = 0;
			}

			//get bumper information from mouse packet
			bumper_status = status_packet.payload.sensors.sensors.bumps_wheeldrops;

			//get ir status information from mouse packet
			ir_status = status_packet.payload.sensors.sensors.wall;

			//mouse bumper is activated and ir sensor aligned
			//if (bumper_status == 1 && ir_status == 1) {
			//if bumper is not activated
			if (bumper_status == 0) {

			}
			//if bumper is activated
			else {
				PORTD ^= _BV(PD6);
				if (ir_status == IR_NORTH) {
					//PORTD ^= _BV(PD4);
					uart_putchar(SENSORS);
					uart_putchar(1);
					while (uart_bytes_received() != 1);

					uint8_t bumps_wheeldrops = uart_get_byte(0);

					if (bumps_wheeldrops == 0) {
						//PORTD ^= _BV(PD4);
					}
					else {
						PORTD ^= _BV(PD4);
					}
				}
			}
		}
	}
	//stop roomba
	//roomba_Drive(0, 0);
}

/**
 * @brief 	Initialize Radio
 */
void init_radio() {
	Radio_Init();

	Radio_Configure_Rx(RADIO_PIPE_0, cat_addr, ENABLE);

	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);

	Radio_Set_Tx_Addr(mouse_addr);
}

/**
 * @brief Set Roomba command
 *
 * @param packet a packet to be sent to Roomba
 * @param command a command to be sent to Roomba
 * @param arguments arguments to be sent to Roomba, first 2 bytes: avg velocity (mm/s), second 2 bytes: radius (mm)
 *
 */
void set_roomba_command(radiopacket_t *packet, uint8_t command, uint8_t arguments[4]) {
	memcpy(command_packet.payload.command.sender_address, cat_addr, 5); //copy sender address to packet
	packet->type = COMMAND;
	packet->payload.command.command = command;
	packet->payload.command.arguments[0] = arguments[0];
	packet->payload.command.arguments[1] = arguments[1];
	packet->payload.command.arguments[2] = arguments[2];
	packet->payload.command.arguments[3] = arguments[3];
	packet->payload.command.num_arg_bytes = 4;
}

void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
}

#endif
